#include <buola/buola.h>

#if defined BUOLA_HAVE_FREENECT && !defined BUOLA_HAVE_OPENNI && !defined BUOLA_HAVE_OPENNI2

#include "ckinectdevice_freenect.h"
#include "ckinectdriver_freenect.h"

#include <buola/image/format.h>
#include <buola/image/io.h>

namespace buola { namespace cvi {

CKinectDevice::CKinectDevice(freenect_context *pContext,int pDeviceNum)
    :   mContext(pContext)
    ,   mDeviceNum(pDeviceNum)
    ,   mInitialized(false)
    ,   mSyncIndex(0)
    ,   mStartCount(0)
    ,   mStarted(false)
{
}

CKinectDevice::~CKinectDevice()
{
    
}

void CKinectDevice::Start()
{
    if(!mStartCount++)
    {
        lock_guard<mutex> lLock(mMutex);
        if(mThread.get_id()==thread::id())
            mThread=thread([this](){OnThread();});
        mCondVar.notify_one();
    }
}

void CKinectDevice::Stop()
{
    mStartCount--;
}

std::pair<img::CImage_rgb8,img::CImage_gray32f> CKinectDevice::GetOne()
{
    std::pair<img::CImage_rgb8,img::CImage_gray32f> lResult;
    Start();
    {
        unique_lock<mutex> lLock(mSyncMutex);
        int lSyncIndex=mSyncIndex;
        while(lSyncIndex==mSyncIndex)
            mSyncCondVar.wait(lLock);
        lResult.first=share(mSyncImageRGB);
        lResult.second=share(mSyncImageDepth);
    }
    Stop();
    
    return lResult;
}

void depth_cb(freenect_device *pDevice,void *pDepth,uint32_t pTimestamp)
{
    //CKinectDevice *lDevice=reinterpret_cast<CKinectDevice*>(freenect_get_user(pDevice));
    
    //uint16_t *lDepthData=(uint16_t*)pDepth;
    
    msg_info() << "got depth!\n";
    
    for(int i=0;i<640*480;i++)
    {
        
    }
}

void video_cb(freenect_device *pDevice,void *pRGB,uint32_t pTimestamp)
{
    //CKinectDevice *lDevice=reinterpret_cast<CKinectDevice*>(freenect_get_user(pDevice));
    
    msg_info() << "got rgb!\n";    
}

void CKinectDevice::Initialize()
{
    lock_guard<mutex> lLock(mMutex);
    if(mInitialized) return;

    if(freenect_open_device(mContext,&mDevice,mDeviceNum)<0)
        throw XHardware("can't open freenect device");
    
    freenect_set_user(mDevice,this);
    freenect_set_depth_callback(mDevice,depth_cb);
    freenect_set_video_callback(mDevice,video_cb);
    freenect_set_video_mode(mDevice,freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_VIDEO_RGB));
    freenect_set_depth_mode(mDevice,freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_DEPTH_REGISTERED));
    freenect_set_video_buffer(mDevice,mRGBBack.RawBytes());
    
    freenect_start_depth(mDevice);
    freenect_start_video(mDevice);
    
    msg_info() << "initialized!\n";
}

void CKinectDevice::OnThread()
{
    int lFrameCount=0;
    
    while(true)
    {
        if(!mStartCount)
        {
            lFrameCount=0;
            
            unique_lock<mutex> lLock(mMutex);
            
            while(!mStartCount)
            {
                if(mStarted)
                    DoStop();
                
                mCondVar.wait(lLock);
            }
        }
        
        if(!mStarted)
        {
            DoStart();
        }
        
        DoIteration(++lFrameCount);
    }
}

void CKinectDevice::DoStart()
{
    if(!mInitialized)
        Initialize();
    
    mStarted=true;
}

void CKinectDevice::DoStop()
{
    
}

void CKinectDevice::DoIteration(int pCount)
{
    int lResult=freenect_process_events(mContext);
    if(lResult<0&&lResult!=-10)
        throw XHardware("can't process freenect events");

    throw XNotImplemented("freenect stuff");
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif
